﻿using System;
using System.Runtime.CompilerServices;

namespace BattleCore.FixedPoint
{
    [Serializable]
    public struct fp2 : IEquatable<fp2>
    {
        #region Private Fields

        private static fp2 zeroVector = new fp2(0, 0);
        private static fp2 oneVector = new fp2(1, 1);

        private static fp2 rightVector = new fp2(1, 0);
        private static fp2 leftVector = new fp2(-1, 0);

        private static fp2 upVector = new fp2(0, 1);
        private static fp2 downVector = new fp2(0, -1);

        #endregion Private Fields

        #region Public Fields

        public fp x;
        public fp y;

        #endregion Public Fields

        #region Properties

        public static fp2 zero
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return zeroVector; }
        }

        public static fp2 one
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return oneVector; }
        }

        public static fp2 right
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return rightVector; }
        }

        public static fp2 left
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return leftVector; }
        }

        public static fp2 up
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return upVector; }
        }

        public static fp2 down
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return downVector; }
        }

        #endregion Properties

        #region Constructors

        /// <summary>
        /// Constructor foe standard 2D vector.
        /// </summary>
        /// <param name="x">
        /// A <see cref="System.Single"/>
        /// </param>
        /// <param name="y">
        /// A <see cref="System.Single"/>
        /// </param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public fp2(fp x, fp y)
        {
            this.x = x;
            this.y = y;
        }

        /// <summary>
        /// Constructor for "square" vector.
        /// </summary>
        /// <param name="value">
        /// A <see cref="System.Single"/>
        /// </param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public fp2(fp value)
        {
            x = value;
            y = value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Set(fp x, fp y)
        {
            this.x = x;
            this.y = y;
        }

        #endregion Constructors

        #region Public Methods

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Reflect(ref fp2 vector, ref fp2 normal,
            out fp2 result)
        {
            fp dot = Dot(vector, normal);
            result.x = vector.x - ((2f * dot) * normal.x);
            result.y = vector.y - ((2f * dot) * normal.y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Reflect(fp2 vector, fp2 normal)
        {
            fp2 result;
            Reflect(ref vector, ref normal, out result);
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Project(fp2 vector, fp2 onNormal)
        {
            var sqrMag = Dot(onNormal, onNormal);
            if (sqrMag < fpmath.Epsilon)
                return zero;
            else
            {
                var dot = Dot(vector, onNormal);
                return new fp2(onNormal.x * dot / sqrMag,
                    onNormal.y * dot / sqrMag);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Add(fp2 value1, fp2 value2)
        {
            value1.x += value2.x;
            value1.y += value2.y;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Add(ref fp2 value1, ref fp2 value2, out fp2 result)
        {
            result.x = value1.x + value2.x;
            result.y = value1.y + value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Barycentric(fp2 value1, fp2 value2,
            fp2 value3, fp amount1, fp amount2)
        {
            return new fp2(
                fpmath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
                fpmath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Barycentric(ref fp2 value1, ref fp2 value2,
            ref fp2 value3, fp amount1,
            fp amount2, out fp2 result)
        {
            result = new fp2(
                fpmath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
                fpmath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 CatmullRom(fp2 value1, fp2 value2,
            fp2 value3, fp2 value4, fp amount)
        {
            return new fp2(
                fpmath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
                fpmath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CatmullRom(ref fp2 value1, ref fp2 value2,
            ref fp2 value3, ref fp2 value4,
            fp amount, out fp2 result)
        {
            result = new fp2(
                fpmath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
                fpmath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Clamp(fp2 value1, fp2 min, fp2 max)
        {
            return new fp2(
                fpmath.Clamp(value1.x, min.x, max.x),
                fpmath.Clamp(value1.y, min.y, max.y));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Clamp(ref fp2 value1, ref fp2 min, ref fp2 max,
            out fp2 result)
        {
            result = new fp2(
                fpmath.Clamp(value1.x, min.x, max.x),
                fpmath.Clamp(value1.y, min.y, max.y));
        }

        /// <summary>
        /// Returns FP precison distanve between two vectors
        /// </summary>
        /// <param name="value1">
        /// A <see cref="fp2"/>
        /// </param>
        /// <param name="value2">
        /// A <see cref="fp2"/>
        /// </param>
        /// <returns>
        /// A <see cref="System.Single"/>
        /// </returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp Distance(fp2 value1, fp2 value2)
        {
            fp result;
            DistanceSquared(ref value1, ref value2, out result);
            return (fp)fp.Sqrt(result);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Distance(ref fp2 value1, ref fp2 value2, out fp result)
        {
            DistanceSquared(ref value1, ref value2, out result);
            result = (fp)fp.Sqrt(result);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp DistanceSquared(fp2 value1, fp2 value2)
        {
            fp result;
            DistanceSquared(ref value1, ref value2, out result);
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void DistanceSquared(ref fp2 value1, ref fp2 value2,
            out fp result)
        {
            result = (value1.x - value2.x) * (value1.x - value2.x) + (value1.y - value2.y) * (value1.y - value2.y);
        }

        /// <summary>
        /// Devide first vector with the secund vector
        /// </summary>
        /// <param name="value1">
        /// A <see cref="fp2"/>
        /// </param>
        /// <param name="value2">
        /// A <see cref="fp2"/>
        /// </param>
        /// <returns>
        /// A <see cref="fp2"/>
        /// </returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Divide(fp2 value1, fp2 value2)
        {
            value1.x /= value2.x;
            value1.y /= value2.y;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Divide(ref fp2 value1, ref fp2 value2,
            out fp2 result)
        {
            result.x = value1.x / value2.x;
            result.y = value1.y / value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Divide(fp2 value1, fp divider)
        {
            fp factor = 1 / divider;
            value1.x *= factor;
            value1.y *= factor;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Divide(ref fp2 value1, fp divider, out fp2 result)
        {
            fp factor = 1 / divider;
            result.x = value1.x * factor;
            result.y = value1.y * factor;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp Dot(fp2 value1, fp2 value2)
        {
            return value1.x * value2.x + value1.y * value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Dot(ref fp2 value1, ref fp2 value2, out fp result)
        {
            result = value1.x * value2.x + value1.y * value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public override bool Equals(object obj)
        {
            return (obj is fp2) ? this == ((fp2)obj) : false;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool Equals(fp2 other)
        {
            return this == other;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public override int GetHashCode()
        {
            return (int)(x + y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Hermite(fp2 value1, fp2 tangent1,
            fp2 value2, fp2 tangent2, fp amount)
        {
            fp2 result = new fp2();
            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Hermite(ref fp2 value1, ref fp2 tangent1,
            ref fp2 value2, ref fp2 tangent2,
            fp amount, out fp2 result)
        {
            result.x = fpmath.Hermite(value1.x, tangent1.x, value2.x, tangent2.x, amount);
            result.y = fpmath.Hermite(value1.y, tangent1.y, value2.y, tangent2.y, amount);
        }

        public fp magnitude
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                fp result;
                DistanceSquared(ref this, ref zeroVector, out result);
                return fp.Sqrt(result);
            }
        }

        public fp sqrMagnitude
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                fp result;
                DistanceSquared(ref this, ref zeroVector, out result);
                return result;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 ClampMagnitude(fp2 vector, fp maxLength)
        {
            return Normalize(vector) * maxLength;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public fp LengthSquared()
        {
            fp result;
            DistanceSquared(ref this, ref zeroVector, out result);
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Lerp(fp2 value1, fp2 value2, fp amount)
        {
            amount = fpmath.Clamp(amount, 0, 1);

            return new fp2(
                fpmath.Lerp(value1.x, value2.x, amount),
                fpmath.Lerp(value1.y, value2.y, amount));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 LerpUnclamped(fp2 value1, fp2 value2,
            fp amount)
        {
            return new fp2(
                fpmath.Lerp(value1.x, value2.x, amount),
                fpmath.Lerp(value1.y, value2.y, amount));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void LerpUnclamped(ref fp2 value1, ref fp2 value2,
            fp amount, out fp2 result)
        {
            result = new fp2(
                fpmath.Lerp(value1.x, value2.x, amount),
                fpmath.Lerp(value1.y, value2.y, amount));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Max(fp2 value1, fp2 value2)
        {
            return new fp2(
                fpmath.Max(value1.x, value2.x),
                fpmath.Max(value1.y, value2.y));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Max(ref fp2 value1, ref fp2 value2, out fp2 result)
        {
            result.x = fpmath.Max(value1.x, value2.x);
            result.y = fpmath.Max(value1.y, value2.y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Min(fp2 value1, fp2 value2)
        {
            return new fp2(
                fpmath.Min(value1.x, value2.x),
                fpmath.Min(value1.y, value2.y));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Min(ref fp2 value1, ref fp2 value2, out fp2 result)
        {
            result.x = fpmath.Min(value1.x, value2.x);
            result.y = fpmath.Min(value1.y, value2.y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Scale(fp2 other)
        {
            this.x = x * other.x;
            this.y = y * other.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Scale(fp2 value1, fp2 value2)
        {
            fp2 result;
            result.x = value1.x * value2.x;
            result.y = value1.y * value2.y;

            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Multiply(fp2 value1, fp2 value2)
        {
            value1.x *= value2.x;
            value1.y *= value2.y;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Multiply(fp2 value1, fp scaleFactor)
        {
            value1.x *= scaleFactor;
            value1.y *= scaleFactor;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Multiply(ref fp2 value1, fp scaleFactor,
            out fp2 result)
        {
            result.x = value1.x * scaleFactor;
            result.y = value1.y * scaleFactor;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Multiply(ref fp2 value1, ref fp2 value2,
            out fp2 result)
        {
            result.x = value1.x * value2.x;
            result.y = value1.y * value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Negate(fp2 value)
        {
            value.x = -value.x;
            value.y = -value.y;
            return value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Negate(ref fp2 value, out fp2 result)
        {
            result.x = -value.x;
            result.y = -value.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Normalize()
        {
            Normalize(ref this, out this);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Normalize(fp2 value)
        {
            Normalize(ref value, out value);
            return value;
        }

        public fp2 normalized
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                fp2 result;
                fp2.Normalize(ref this, out result);

                return result;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Normalize(ref fp2 value, out fp2 result)
        {
            fp factor;
            DistanceSquared(ref value, ref zeroVector, out factor);
            factor = 1f / (fp)fp.Sqrt(factor);
            result.x = value.x * factor;
            result.y = value.y * factor;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 SmoothStep(fp2 value1, fp2 value2,
            fp amount)
        {
            return new fp2(
                fpmath.SmoothStep(value1.x, value2.x, amount),
                fpmath.SmoothStep(value1.y, value2.y, amount));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void SmoothStep(ref fp2 value1, ref fp2 value2, fp amount,
            out fp2 result)
        {
            result = new fp2(
                fpmath.SmoothStep(value1.x, value2.x, amount),
                fpmath.SmoothStep(value1.y, value2.y, amount));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 Subtract(fp2 value1, fp2 value2)
        {
            value1.x -= value2.x;
            value1.y -= value2.y;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Subtract(ref fp2 value1, ref fp2 value2,
            out fp2 result)
        {
            result.x = value1.x - value2.x;
            result.y = value1.y - value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp Angle(fp2 a, fp2 b)
        {
            return fp.Acos(a.normalized * b.normalized) * fp.Rad2Deg;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public fp3 ToTSVector()
        {
            return new fp3(this.x, this.y, 0);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public override string ToString()
        {
            return string.Format("({0:f1}, {1:f1})", x.AsFloat(), y.AsFloat());
        }

        #endregion Public Methods

        #region Operators

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 operator -(fp2 value)
        {
            value.x = -value.x;
            value.y = -value.y;
            return value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(fp2 value1, fp2 value2)
        {
            return value1.x == value2.x && value1.y == value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(fp2 value1, fp2 value2)
        {
            return value1.x != value2.x || value1.y != value2.y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 operator +(fp2 value1, fp2 value2)
        {
            value1.x += value2.x;
            value1.y += value2.y;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 operator -(fp2 value1, fp2 value2)
        {
            value1.x -= value2.x;
            value1.y -= value2.y;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp operator *(fp2 value1, fp2 value2)
        {
            return fp2.Dot(value1, value2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 operator *(fp2 value, fp scaleFactor)
        {
            value.x *= scaleFactor;
            value.y *= scaleFactor;
            return value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 operator *(fp scaleFactor, fp2 value)
        {
            value.x *= scaleFactor;
            value.y *= scaleFactor;
            return value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 operator /(fp2 value1, fp2 value2)
        {
            value1.x /= value2.x;
            value1.y /= value2.y;
            return value1;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp2 operator /(fp2 value1, fp divider)
        {
            fp factor = 1 / divider;
            value1.x *= factor;
            value1.y *= factor;
            return value1;
        }

        #endregion Operators
    }
}